
#include "Object.h"
#include <osgDB/ReadFile>
#include <osgDB/WriteFile>
#include "CommonTypes.h"
using namespace IG;
int Object::id = 1;;
Object::Object(char* filename)
{
	transform = new osg::MatrixTransform;
	myId = id++;
	loadedModel = osgDB::readNodeFile(filename);

	if (loadedModel.valid())
	{
	//	transform->addChild(loadedModel.get());
		AddSensorNode(transform->asGroup(), loadedModel->asGroup(), GRAY, 0.25, 0.0);
		AddSensorNode(transform->asGroup(), loadedModel->asGroup(), RGB, 1.0, 0.0);		
		AddSensorNode(transform->asGroup(), loadedModel->asGroup(), IR, 0.0, 0.0);
	}
	nameNodes.pnode = NULL;
	transform->accept(nameNodes);
	if (nameNodes.pnode != NULL)
	{
		osg::Group* g1 = nameNodes.pnode->asGroup();
		osg::Node* gIR = g1->getChild(0);
		
		

		AddSensorNode(g1, gIR->asGroup(), RGB, 1.0, 0.0);
		AddSensorNode(g1, gIR->asGroup(), GRAY, 1.0, 1.0);
		AddSensorNode(g1, gIR->asGroup(), IR, 1.0, 1.0);
		g1->removeChild(gIR);


	}
	//slaveCamera = NULL;
}
void Object::AddSensorNode(osg::Group* father, osg::Group* child, CAMERA_SENSOR kind, float maxInt, float minInt)
{

	osg::Group* sensorG = new osg::Group();

	osg::StateSet* stt = sensorG->getOrCreateStateSet();
	stt->addUniform(new osg::Uniform("maxIntensity", maxInt));
	stt->addUniform(new osg::Uniform("minIntensity", minInt));
	int uk = (int)kind;
	stt->addUniform(new osg::Uniform("sensor", uk));

	if (kind == CAMERA_SENSOR::RGB)	
	{
		sensorG->setNodeMask(CCD_SENSOR_MASK);
		sensorG->setName("ccd_Sensor");
		
	}
	else
	{
		if (kind == CAMERA_SENSOR::GRAY)
		{
			sensorG->setNodeMask(GRAY_SENSOR_MASK);
			sensorG->setName("gray_Sensor");
			
		}
		else
		{
			sensorG->setNodeMask(IR_SENSOR_MASK);
			sensorG->setName("ir_Sensor");
			
		}
	}

	
	
	father->addChild(sensorG);
	sensorG->addChild(child);
}
 Object::~Object()
{
}
 void Object::SetObjectPosition(osg::Vec3d position, osg::Vec3d orientation)
 {
	 osg::Matrix viewMatrix = osg::Matrix::rotate(osg::DegreesToRadians(orientation.z()), 0.0, 1.0, 0.0)*			//roll
		 osg::Matrix::rotate(osg::DegreesToRadians(orientation.y()), 1.0, 0.0, 0.0)*		//pitch
		 osg::Matrix::rotate(osg::DegreesToRadians(orientation.x()), 0.0, 0.0, 1.0)*
		 osg::Matrix::translate(position);
	 //transform->setMatrix(		//osg::Matrix::translate(position)*
		// //osg::Matrix::scale(size, size, size)*
		//	osg::Matrix::rotate(osg::DegreesToRadians(orientation.z()), 0.0, 1.0, 0.0)*			//roll
		//	osg::Matrix::rotate(osg::DegreesToRadians(orientation.y()), 1.0, 0.0, 0.0)*		//pitch
		//	osg::Matrix::rotate(osg::DegreesToRadians(orientation.x()), 0.0, 0.0, 1.0)*
		//	osg::Matrix::translate(position));//heading
	 transform->setMatrix(viewMatrix);
	 osg::Matrix r = osg::Matrix::rotate(osg::DegreesToRadians(orientation.z()), 0.0, 1.0, 0.0)*			//roll
		 osg::Matrix::rotate(osg::DegreesToRadians(orientation.y()), 1.0, 0.0, 0.0)*		//pitch
		 osg::Matrix::rotate(osg::DegreesToRadians(orientation.x()), 0.0, 0.0, 1.0);
		 
	 //if (slaveCamera != NULL) // check if valid is NULL
	 //{
		//slaveCamera->UpdatePosition(viewMatrix,r);
	 //}
	 if (slaveCameraList.size() > 0)
	 {
		 slaveCamItr itr = slaveCameraList.begin();
		 for (; itr != slaveCameraList.end(); itr++)
		 {
			 (*itr)->UpdatePosition(viewMatrix, r);;
		 }
	 }

 }
 void Object::SetSlaveCamera(IG::Camera* cam)
 {
	 slaveCameraList.push_back(cam);
	 //slaveCamera = cam;
	 
 }